function [fusion] = fcn_fusion_ecf(gyro, accb, magb, stoch_p, q_initial, pos_initial, no_int, KPR, T1, acc1g, dt)
%fcn_fusion_ecf     This function uses Robert Mahony's Explicit Complementary
%                   Filter as sensor fusion algorithm.

%   [fusion] = fcn_fusion_ecf(gyro, accb, magb, stoch_p, q_initial, vel_initial, pos_initial, no_int, KPR, T1, acc1g, dt)
%   This function uses Robert Mahony's Explicit Complementary Filter as
%   sensor fusion algorithm. The algorithm was first introduced in the
%   paper "Nonlinear Complementary Filters on the Special Orthogonal Group"
%   back in 2008. Meanwhile a quaternion based and even a control-coupled
%   version of this filter is available. For further information please
%   refer to "A coupled estimation and control analysis for attitude 
%   stabilisation of mini aerial vehicles".

%   IN      gyro        Gyro measurements error-prone
%   IN      accb        Acc measurements error-prone
%   IN      magb        Mag measurements error-prone
%   IN      stoch_p     Vector containing stochastic parameters for error modells
%   IN      q_initial   Initial quaternion attitude
%   IN      pos_initial Initial position
%   IN      no_int      Number of samples before position integration starts (time for the algorithm to reach a steady state)
%   IN      KPR         Controller parameter
%   IN      T1          Controller parameter
%   IN      acc1g       Gravity (equals the gravity at Analog Devices Calibration site)
%   IN      dt          Sample period
%   OUT     fusion      Fusioned data


%% Get QUEST initial attitude if wanted
if strcmpi(q_initial, 'QUEST')
	quest_noise = [stoch_p.Nacc; stoch_p.Nmag];
    obs = [ [accb(1,:)/norm(accb(1,:))]', [magb(1,:)/norm(magb(1,:))]'];
    ref = [[0, 0, 1]', [1, 0, 0]'];
    q_initial = quest(obs, ref, quest_noise, 'noise vectors', 1);    
end

%% For algorithm
% Get loop length
k = length(gyro);

% Alloc
e_acc       = zeros(1, 3);                      % crossproduct fusion parts
e_mag       = zeros(1, 3);
e_n         = zeros(1, 3);                      % current crossproduct between estimated and measured vectors
e_n1        = zeros(1, 3);                      % former crossproduct
gyro_corr_n = zeros(1, 3);                      % current gyro correction from acc/mag
gyro_corr_n1= zeros(1, 3);                      % former gyro correction

% Save for output
pos_save = zeros(k, 3);                         % position
vel_save = zeros(k, 3);                         % velocity
q_save   = zeros(k, 4);                         % quaternion attitude
gyro_corr_save = zeros(k, 3);                   % corrected gyro data

% Algorithm initials
x = KPR * ((2*T1 + dt) / (2*T1));
y = KPR * ((2*T1 - dt) / (2*T1));

% Initials
q = q_initial;
v = zeros(1, 3);
p = pos_initial;


for j=1:k,j; 
    
    % Get current data for the sake of readability...
    wm = gyro(j,:);
    am = accb(j,:);
    mm = magb(j,:);

    % Normalize vectors for attitude determination
    accb_norm = am / norm(am);
    magb_norm = mm / norm(mm);

	%% Mag fusion
	% transfer mag vector to NED (inertial) frame
	mag_earth = quatmultiply(q, quatmultiply([0 magb_norm], quatconj(q)));
        
    % normalize x and y component -> vector which gets turned to north
    % (length maintained!!!)
    b = [0 norm([mag_earth(2) mag_earth(3)]) 0 mag_earth(4)];

    % estimate field from quaternion and b (transfer b back to
    % bodyframe)
    w = [2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))
         2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))
         2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)];  

    %% Acc fusion
    % estimate gravity from quaternion (basically 3rd column of rotation matrix)
    g = [2 * (-q(1) * q(3) + q(2) * q(4))
         2 * (q(1) * q(2) + q(3) * q(4))
         q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2]; 
       
    % Get error vector as cross product between measured gravity and calculated gravity from quaternion
    % and error from measured earth field and calculated earth field
    e_acc(1) = accb_norm(2)*g(3) - accb_norm(3)*g(2);
    e_acc(2) = accb_norm(3)*g(1) - accb_norm(1)*g(3);
    e_acc(3) = accb_norm(1)*g(2) - accb_norm(2)*g(1);

    e_mag(1) = magb_norm(2)*w(3) - magb_norm(3)*w(2);
    e_mag(2) = magb_norm(3)*w(1) - magb_norm(1)*w(3);
    e_mag(3) = magb_norm(1)*w(2) - magb_norm(2)*w(1);

    e_n = e_acc + e_mag;
     
    %% PI gyro correction
    gyro_corr_n = x * e_n - y * e_n1 + gyro_corr_n1;
    gyro_corr_n1 = gyro_corr_n;
    e_n1 = e_n;

    % Tune gyro
    wm = wm + gyro_corr_n;
    gyro_corr_save(j,:) = wm;
    
	% Propagate quaternion
    q = dq_multi_trig(q, wm, dt, 'norm');

    % Save for output
    q_save(j,:) = q;
    
    %% ONLY start position integration if ss_time is over (algorithm and
    % attitude reached steady state)
    if (j > no_int)
        % Velocity and Position
        % Transfer acc vector to NED (inertial) frame
        acc_earth = quatmultiply((q), quatmultiply([0 am], quatconj(q)));
        acc_earth = acc_earth(1, 2:4);

        % Substract 1g from z-component
        acc_earth(3) = acc_earth(3) - acc1g;

        % Integrate to velocity
        v = v + acc_earth * dt;
        
        % Save for output
        vel_save(j,:) = v;

        % Integrate to position
        p = p + v * dt;

        % Save for output
        pos_save(j,:) = p;
    
    else
        % Save for output        
        vel_save(j,:) = v;
        pos_save(j,:) = p;
   
    end

end


%% Output
% Eulerangles from quaternion attitude
[yaw, pitch, roll] = quat2angle( q_save, 'ZYX' );

% Transform vectors to inertial frame
tmp(:,2:4) = accb;
acci_save = quatmultiply(q_save, quatmultiply(tmp, quatconj(q_save)));
tmp(:,2:4) = magb;
magi_save = quatmultiply(q_save, quatmultiply(tmp, quatconj(q_save)));

% Output sample time
for j=1:k,j;
    fusion.time(j,1) = j*dt;
end

% Output position
fusion.X = pos_save(:,1);
fusion.Y = pos_save(:,2);
fusion.Z = pos_save(:,3);
fusion.pos = pos_save;

% Output velocity
fusion.vel = vel_save;
fusion.v_xy = (vel_save(:,1).^2 + vel_save(:,2).^2).^(1/2);

% Output acceleration
fusion.a_xy = (acci_save(:,2).^2 + acci_save(:,3).^2).^(1/2);

% Output euler angles
fusion.phi = roll;
fusion.theta = pitch;
fusion.psi = yaw;

% Output quaternion attitude
fusion.q = q_save;

% Output quaternion attitude where position integration starts
fusion.q_int = q_save(no_int+1,:);

% Output measured and corrected gyro rates
fusion.gyro = gyro;
fusion.gyro_corr = gyro_corr_save;

% Output direction vectors: accerlation
fusion.acci = acci_save(:,2:4);

% Output direction vectors: magnetometer
fusion.magi = magi_save(:,2:4);

end

